#ifndef SIMULATOR_LIDAR_NODE_H
#define SIMULATOR_LIDAR_NODE_H

#include "rclcpp/rclcpp.hpp"


#include "sensor_msgs/msg/laser_scan.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "tf2/LinearMath/Transform.h"
#include <geometry_msgs/msg/transform_stamped.hpp>
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"

#define ERROR          -2
#define UNKNOWN_SPACE  -1
#define FREE_SPACE     0
#define OBS_SPACE      100

#define esp 1e-6


namespace simulator {
    class SimulatorLidar : public rclcpp::Node {
    public:
        SimulatorLidar();
        ~SimulatorLidar() override;


    private:
        double normalAngle(double theta);
        bool limit_range(int& mx, int& my);

        double gaussRand(double mu, double sigma);

        void mapToWorld(int mx, int my, float& wx, float& wy);
        void worldToMap(float wx, float wy, int& mx, int& my);

        char getValue(int mx, int my);
        char getValue(float wx, float wy);

        void initLaserScan(sensor_msgs::msg::LaserScan& laser_scan);
        void getFrame(sensor_msgs::msg::LaserScan& laser_scan);

        void getPose(geometry_msgs::msg::TransformStamped& transform, double& start_angle, double& reverse);

        void distance(int start_x, int start_y, double theta, int& end_x, int& end_y);
        void distance(float start_x, float start_y, double theta, float& end_x, float& end_y);

        void mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr grid_map);
        void pubLaserCallback();
        void odomCallback(const nav_msgs::msg::Odometry::SharedPtr odom_msg);

        std::string global_frame_, lidar_frame_, stage_map_topic_, odom_topic_;
        double min_angle_, max_angle_, step_;
        double min_dis_, max_dis_;
        double noise_;
        int point_size_;
        int rate_;
        bool use_topic_odom_;

        unsigned int data_length_;
        nav_msgs::msg::OccupancyGrid local_map_;
        nav_msgs::msg::Odometry odom_msg_;


        std::shared_ptr<tf2_ros::Buffer> tf_;
        std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
        
        rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr lidar_pub_;
        rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
        rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
        rclcpp::TimerBase::SharedPtr pub_laser_timer_;
    };
}

#endif // SIMULATOR_LIDAR_NODE_H
